/*
 *             _..--""-.                  .-""--.._
 *         _.-'         \ __...----...__ /         '-._
 *       .'      .:::...,'              ',...:::.      '.
 *      (     .'``'''::;                  ;::'''``'.     )
 *       \             '-)              (-'             /
 *        \             /                \             /
 *         \          .'.-.            .-.'.          /
 *          \         | \0|            |0/ |         /
 *          |          \  |   .-==-.   |  /          |
 *           \          `/`;          ;`\`          /
 *            '.._      (_ |  .-==-.  | _)      _..'
 *                `"`"-`/ `/'        '\` \`-"`"`
 *                     / /`;   .==.   ;`\ \
 *               .---./_/   \  .==.  /   \ \
 *              / '.    `-.__)       |    `"
 *             | =(`-.        '==.   ;
 *              \  '. `-.           /
 *               \_:_)   `"--.....-'
 *
 *               file: control_node.cpp
 *               description: node handle to convert force to gripper movement.
 *               authors: Michiel and Peter
 */

#include <ros/ros.h>
#include "gripper/control_node.h"

/**
 * Get sensor input.
 * @param	sensor	int32 presenting the sensor value.
 */
void ControlNode::sensorCallback(const std_msgs::Int32::ConstPtr &sensor)
{
	sensor_value = sensor->data;
}

/**
 * Init motor, subscribe to force topic.
 */
void ControlNode::init()
{
	// subscribe to listener
	sensor_ = nh_.subscribe(Topic::IR_DISTANCE, 1000, &ControlNode::sensorCallback, this);
	motor_pub_ = nh_.advertise<std_msgs::Int32>(Topic::MOTOR, 1);
}

/**
 * Check if there is an object in reach and grab it.
 */
void ControlNode::spin()
{
	ros::Rate loop_rate(10);	// hertz
	std_msgs::Int32 state;

	while(ros::ok())
	{
		if(sensor_value >= 0 && sensor_value <= (GripperDefines::MIN_DISTANCE * 10))
		{
			state.data = GripperDefines::CLOSED;		// sluit gripper
			motor_pub_.publish(state);
		}
		else
		{
			state.data = GripperDefines::OPEN;	// open gripper
			motor_pub_.publish(state);
		}


		if(state.data == GripperDefines::CLOSED)
			ROS_INFO("dicht");
		else
			ROS_INFO("open");

		ros::spinOnce();
		loop_rate.sleep();
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "control_node");	// node name calc_node
	ControlNode control;
	control.init();
	control.spin();

	return 0;
}
